import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from transforms3d.euler import quat2euler

class TFListener(Node):
    def __init__(self):
        super().__init__('tf2_listener')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.timer = self.create_timer(1, self.get_transform)
        
    def get_transform(self):
        try:
            tf = self.buffer.lookup_transform('map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
            transform = tf.transform
            rotation_eular = quat2euler([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w,
            ])
            self.get_logger().info(f'translation:{transform.translation},quaternion:{transform.rotation},eylar:{rotation_eular}')
        except Exception as e:
            self.get_logger().warn(f'不能获取坐标变换， 原因：{str(e)}')
            # Exception是一个基类
            # 平时我们用 except Exception as e 时，
            # 捕获的是解释器自动生成的异常实例，所以不需要自己手动 new 一个；
            # 只有主动抛异常时，才需要显式创建实例。
            
def main():
    rclpy.init()
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()